﻿using System;
using Microsoft.Kinect;
using System.Diagnostics;

namespace K4Wv2PointCloud
{
    /// <summary>
    /// Point Cloud Argument of a PointCloud Ready Event.
    /// </summary>
    public class PointCloudReadyEventArg : EventArgs {
        /// <summary>
        /// Gets or sets the reference(!) to point cloud.
        /// </summary>
        /// <value>
        /// The point cloud.
        /// </value>
        public CameraSpacePoint[] PointCloud { get; set; }
    }

    /// <summary>
    /// Kinect V2 reader to retrieve Point Clouds.
    /// <para>
    /// Register an Eventhandler, call Initialize() and let it work. Call Stop() to end reading from kinect and
    /// Initialize() to start again.
    /// </para>
    /// </summary>
    public class K4Wv2PointCloud
    {
        #region Fields

        /// <summary>Active Kinect sensor</summary>
        static volatile KinectSensor _kinectSensor = null;
        /// <summary>lock object</summary>
        static object _syncRoot = new Object();
        /// <summary>Reads DepthFrames from Kinect.</summary>
        DepthFrameReader _reader;
        /// <summary>Buffer to hold the depth frame data (16Bit).</summary>
        ushort[] _depthFrameData;

        #endregion


        #region Properties & Events

        /// <summary>
        /// Occurs when new Point Cloud is Ready.
        /// </summary>
        public event EventHandler<PointCloudReadyEventArg> PointCloudReady;

        /// <summary>
        /// Gets the (a) KinectSensor Device.
        /// </summary>
        /// <value>
        /// The kinect or null, if no divice found / connected.
        /// </value>
        public KinectSensor Kinect {
            get {
                if (_kinectSensor == null) {
                    lock (_syncRoot) {
                        if (_kinectSensor == null) {
                            // for Alpha, one sensor is supported
                            _kinectSensor = KinectSensor.Default;
                        }
                    }
                }
                return _kinectSensor;
            }
        }

        /// <summary>
        /// Gets the point cloud.
        /// </summary>
        /// <value>
        /// The point cloud.
        /// </value>
        public CameraSpacePoint[] PointCloud { get; private set; }

        #endregion


        #region Constructors

        /// <summary>
        /// Initializes a new instance of the <see cref="K4Wv2PointCloud"/> class.
        /// <para>
        /// Register an Eventhandler, call Initialize() and let it work. Call Stop() to end reading from kinect and
        /// Initialize() to start again.
        /// </para>
        /// </summary>
        public K4Wv2PointCloud() {
            //Initialize();
        }

        #endregion

        #region Methods

        public bool Initialize() {
            var kinect = Kinect;
            if (kinect != null) {
                kinect.Open();
                // open the reader for the depth frames
                _reader = kinect.DepthFrameSource.OpenReader();
                // allocate space to put the pixels being received and converted
                FrameDescription frameDescription = kinect.DepthFrameSource.FrameDescription;
                _depthFrameData = new ushort[frameDescription.Width * frameDescription.Height];
                PointCloud = new CameraSpacePoint[_depthFrameData.Length];
                // register Event Handler for reading data.
                _reader.FrameArrived += this.Reader_FrameArrived;
                return true;
            }
            else {
                Trace.TraceWarning("Failed to initialize Kinect Sensor.");
                return false;
            }
            
        }

        /// <summary>
        /// Stops the Kinect.
        /// </summary>
        public void Stop() {
            var kinect = Kinect;
            if (kinect != null) {
                _reader.FrameArrived -= this.Reader_FrameArrived;
                _reader.Dispose();
                kinect.Close();
            }
        }

        /// <summary>
        /// Handles the FrameArrived event of the Reader control.
        /// </summary>
        /// <param name="sender">The source of the event.</param>
        /// <param name="e">The <see cref="DepthFrameArrivedEventArgs"/> instance containing the event data.</param>
        /// <exception cref="System.NotImplementedException"></exception>
        private void Reader_FrameArrived(object sender, DepthFrameArrivedEventArgs e) {
            DepthFrameReference frameReference = e.FrameReference;

            try {
                DepthFrame frame = frameReference.AcquireFrame(); // read depth-frame

                if (frame != null) { // maybe referenced frame overwritten already
                    using (frame) {
                        // Copy the depth data from the frame to a temporary array
                        frame.CopyFrameDataToArray(_depthFrameData);
                        // Map the depthPixel data to point cloud
                        Kinect.CoordinateMapper.MapDepthFrameToCameraSpace(_depthFrameData, PointCloud);
                        // throw Event
                        if (PointCloudReady != null) {
                            PointCloudReady(this, new PointCloudReadyEventArg { PointCloud = this.PointCloud });
                        }
                    }
                }
            }
            catch (Exception) {
                Trace.TraceError("Failure at K4Wv2PointCloud.Reader_FrameArrived");
            }
        }

        #endregion
 
    }
}
